#include <iostream>
#include <fstream>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>

using namespace std;
using namespace Sophus;

string groundtruth_file="./example/groundtruth.txt";
string estimated_file= "./example/estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajactory(const TrajectoryType &gt, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char* argv[])
{
    TrajectoryType groundtruth=ReadTrajectory((groundtruth_file);
    TrajectoryType estimated=ReadTrajectory((estimated_file);
    assert(!groundtruth.empty()&&!estimated.empty());
    assert(groundtruth.size()==estimated.size());
    
    double rms=0;  //计算误差；
    for(size_t i=0;i<estimated.size();i++)    //绝对误差法
    {
        Sophus::SE3d p1=estimated[i],p2=groundtruth[i];
        double error=(p2.inverse()*P1).log().norm();
        rms+=error*error;
    }
    
    rmse=rms/double(estimated.size());
    rmse=sqrt(rmse);
    cout<< "RMSE = "<<rms<<endl;
    
    DrawTrajactory(groundtruth,estimated);
    
    return 0;
}

TrajectoryType ReadTrajectory(const string &path)   //返回vector类型；
{
    ifstream fin(path);
    TrajectoryType trajectory;
    if(!fin)
    {
        cerr<<"trajectory "<<path <<" not found "<<endl;
    
    }
    return trajectory;
    
    while(!fin.eof())
    {
        double time, tx, ty ,tz, qx, qy, qz, qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;   //位姿用四元数保存；
        Sophus::SE3d p1(Eigen::Quaterniond(qx,qy,qz,qw),Eigen::vector3d(tx,ty,tz));
        trajectory.push_back(p1);  //保存形式为SE3d,与Vextor3d
    }
    
    return trajectory;
}

void DrawTrajactory(Vector<Isometray3d,Eigen::aligned_allocator<Isometray3d>> poses)
{
    //创建pangolin窗口
    
    pangolin::CreateWindowAndBind("trajectory view",1024,768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectMatrix(1024,768,500,500,512,389,0.1,1000),
        pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
                                );
    pangolin::view &d_cam = pangolin::CreateDsiplay().setBounds(0.0,1.0,0.0,1.0,-1024.0f/167.0f).setHandler(new pangolin::Handler3D(s_cam));
    
    while (pangolin::ShouldQuit()==false)
    {
        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        glLineWidth(2);
        for(size_t i=0;i<poses.size();i++)
        {
            Vector3d Ow=poses[i].translation();
            Vector3d Xw=poses[i]*(0.1*Vector3d(1,0,0));
            Vector3d Yw=poses[i]*(0.1*Vector3d(0,1,0));
            Vector3d Zw=poses[i]*(0.1*Vector3d(0,0,1));
            
            glBegin(GL_LINES);
            glColor3f(1.0,0.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Xw[0],Xw[1],Xw[2]);
            glColor3f(0.0,1.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Yw[0],Yw[1],Yw[2]);
            glColor3f(0.0,1.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Zw[0],Zw[1],Zw[2]);
            
            glEnd();
        }
        
        for(size_t i=0;i<poses.size();i++)
        {
            glColor3f(0.0,0.0,0.0);
            glBegin(GL_LINES);
            auto p1=poses[i],p2=poses[i+1];
            glVertex3d(p1.translation()[0],p1.translation()[1],p1.translation()[2]);
            glVertex3d(p2.translation()[0],p2.translation()[1],p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);
    }
}
            
        

        








        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
